/*
 * Homography.hpp
 *
 *  Created on: Apr 24, 2012
 *      Author: fdesorbi
 *     Contact: fdesorbi@hvrl.ics.keio.ac.jp
 */

#ifndef _HOMOGRAPHY_HPP__
#define _HOMOGRAPHY_HPP__

#include <opencv2/opencv.hpp>

namespace hvrl
{

namespace Homography
{

/**
 * @brief Compute the Jacobian \f$\frac{\partial{\mathbf{W}}}{\partial{\mathbf{p}}}\f$ of a homography with parameters \f$\mathbf{p}\f$ and evaluated at \f$(\mathbf{x};\mathbf{0})\f$ with \f$\mathbf{x}=(x,y)\f$
 *
 * The Jacobian \f$\frac{\partial{\mathbf{W}}}{\partial{\mathbf{p}}}\f$ at \f$(\mathbf{x};\mathbf{0})\f$ is:
 *
 * \f$ \frac{\partial{\mathbf{W}}}{\partial{\mathbf{p}}} = \left( \begin{array}{cccccccc} x & y & 1 & 0 & 0 & 0 & -xx & -yx \\ 0 & 0 & 0 & x & y & 1 & -xy & -yy \end{array} \right)\f$
 *
 * The Jacobian is a \f$(16\times (width\times height))\f$ matrix where each line is the derivate first over x then over y
 *
 * @param size Size of the input data
 * @param output
 * @author Francois de Sorbier
 */
void jacobianLines(const cv::Size& size, cv::Mat& output);

/**
 * @brief Compute the Jacobian \f$\frac{\partial{\mathbf{W}}}{\partial{\mathbf{p}}}\f$ of a homography with parameters \f$\mathbf{p}\f$ and evaluated at \f$(\mathbf{x};\mathbf{0})\f$ with \f$\mathbf{x}=(x,y)\f$
 *
 * The Jacobian \f$\frac{\partial{\mathbf{W}}}{\partial{\mathbf{p}}}\f$ at \f$(\mathbf{x};\mathbf{0})\f$ is:
 *
 * \f$ \frac{\partial{\mathbf{W}}}{\partial{\mathbf{p}}} = \left( \begin{array}{cccccccc} x & y & 1 & 0 & 0 & 0 & -xx & -yx \\ 0 & 0 & 0 & x & y & 1 & -xy & -yy \end{array} \right)\f$
 *
 * The Jacobian is a \f$((8\times width)\times (2\times height))\f$ matrix where each line is the derivate first over x then over y
 *
 * @param size Size of the input data
 * @param output
 * @author Francois de Sorbier
 */
void jacobianBlocks(const cv::Size& size, cv::Mat& output);


/**
 * @brief Compute the Steepest Descent Image defined as \f$\nabla \mathbf{T} \frac{\partial{\mathbf{W}}}{\partial{\mathbf{p}}}\f$ where \f$\nabla \mathbf{T}\f$ is the gradient of the image \f$\mathbf{T}\f$
 *
 * The steepest descent image is computed in a line version, meaning that the Jacobian is also defined as lines (the derivative over one parameter is defined on one line)
 *
 * @param gradientX The gradient of an image over the x-axis
 * @param gradientY The gradient of an image over the y-axis
 * @param jacobian The Jacobian defined on lines for a homography
 * @param output The steepest descent image result
 * @author Francois de Sorbier
 */
void steepestDescentImageLines(const cv::Mat& gradientX, const cv::Mat& gradientY, const cv::Mat& jacobian, cv::Mat& output);

/**
 * @brief Compute the Steepest Descent Image defined as \f$\nabla \mathbf{T} \frac{\partial{\mathbf{W}}}{\partial{\mathbf{p}}}\f$ where \f$\nabla \mathbf{T}\f$ is the gradient of the image \f$\mathbf{T}\f$
 *
 * The steepest descent image is computed in a block version, meaning that the Jacobian is also defined as blocks
 *
 * @param gradientX The gradient of an image over the x-axis
 * @param gradientY The gradient of an image over the y-axis
 * @param jacobian The Jacobian defined on blocks for a homography
 * @param output The steepest descent image result
 * @author Francois de Sorbier
 */
void steepestDescentImageBlocks(const cv::Mat& gradientX, const cv::Mat& gradientY, const cv::Mat& jacobian, cv::Mat& output);

/**
 * @brief Compute the Hessian defined as \f$\displaystyle\sum_{\mathbf{x}} [\nabla \mathbf{T} \frac{\partial{\mathbf{W}}}{\partial{\mathbf{p}}}]^T [\nabla \mathbf{T} \frac{\partial{\mathbf{W}}}{\partial{\mathbf{p}}}]\f$
 *
 * @param steepestdescentimage The steepest descent image
 * @param ouput The Hessian
 * @author Francois de Sorbier
 */
void hessianLines(const cv::Mat& steepestdescentimage, cv::Mat& ouput);


/**
 * @brief Compute the Hessian defined as \f$\displaystyle\sum_{\mathbf{x}} [\nabla \mathbf{T} \frac{\partial{\mathbf{W}}}{\partial{\mathbf{p}}}]^T [\nabla \mathbf{T} \frac{\partial{\mathbf{W}}}{\partial{\mathbf{p}}}]\f$
 *
 * This hessian is computed for an homography evaluated at \f$(\mathbf{x};\mathbf{0})\f$ with \f$\mathbf{x}=(x,y)\f$
 * The function is optimized for avoiding extra computations and is then faster than using sub-function separately.
 *
 * @param gradientX The gradient of an image over the x-axis
 * @param gradientY The gradient of an image over the y-axis
 * @param hessian The Hessian from a Jacobian evaluated at \f$(\mathbf{x};\mathbf{0})\f$
 * @param steepestdescentimage The Steepest Descent Image from a Jacobian evaluated at \f$(\mathbf{x};\mathbf{0})\f$ in a line form
 * @author Francois de Sorbier
 */
template <typename T>
void hessian(const cv::Mat& gradientX, const cv::Mat& gradientY, cv::Mat& hessian, cv::Mat& steepestdescentimage)
{
	hessian = cv::Mat(8, 8, cv::DataType<T>::type, 0.0f);
	steepestdescentimage = cv::Mat(gradientX.rows*gradientX.cols, 8, cv::DataType<T>::type, 0.0f);

	T *SDI = 0;

	for (unsigned int i = 0; i < gradientX.rows; ++i)
	{
		for (unsigned int j = 0; j < gradientX.cols; ++j)
		{
			SDI = steepestdescentimage.ptr<T>(i*gradientX.cols + j);
			SDI[0] = gradientX.at<T> (i, j) * j;
			SDI[1] = gradientX.at<T> (i, j) * i;
			SDI[2] = gradientX.at<T> (i, j);
			SDI[3] = gradientY.at<T> (i, j) * j;
			SDI[4] = gradientY.at<T> (i, j) * i;
			SDI[5] = gradientY.at<T> (i, j);
			SDI[6] = -gradientX.at<T> (i, j) * j * j
					- gradientY.at<T> (i, j) * i * j;
			SDI[7] = -gradientY.at<T> (i, j) * i * i
					- gradientX.at<T> (i, j) * i * j;

			for (unsigned int k = 0; k < 8; ++k)
				for (unsigned int l = 0; l < 8; ++l)
					hessian.at<T> (k, l) += SDI[k] * SDI[l];
		}
	}

	steepestdescentimage = steepestdescentimage.t();

}


/**
 * @brief Compute the inverse of a homography
 *
 * @param h The input homography
 * @param hinv The inverse of the input (if it exists)
 * @return true if the inverse was correctly computed
 * @author Francois de Sorbier
 */
template <typename T>
bool inverse(const cv::Mat& h, cv::Mat& hinv)
{
	if(h.rows!=3 || h.cols!=3)
			return false;
	// Compute the determinant of the homography
	float determinant = h.at<T>(0,0)*(h.at<T>(1,1)*h.at<T>(2,2)-h.at<T>(2,1)*h.at<T>(1,2)) +
						h.at<T>(1,0)*(h.at<T>(2,1)*h.at<T>(0,2)-h.at<T>(0,1)*h.at<T>(2,2)) +
						h.at<T>(2,0)*(h.at<T>(0,1)*h.at<T>(1,2)-h.at<T>(1,1)*h.at<T>(0,2));

	if(determinant == 0.0f){
		return false;
	}

	hinv.at<T>(0,0) = determinant*(h.at<T>(1,1)*h.at<T>(2,2)-h.at<T>(2,1)*h.at<T>(1,2));
	hinv.at<T>(0,1) = determinant*(h.at<T>(0,2)*h.at<T>(2,1)-h.at<T>(2,2)*h.at<T>(0,1));
	hinv.at<T>(0,2) = determinant*(h.at<T>(0,1)*h.at<T>(1,2)-h.at<T>(1,1)*h.at<T>(0,2));

	hinv.at<T>(1,0) = determinant*(h.at<T>(1,2)*h.at<T>(2,0)-h.at<T>(2,2)*h.at<T>(1,0));
	hinv.at<T>(1,1) = determinant*(h.at<T>(0,0)*h.at<T>(2,2)-h.at<T>(2,0)*h.at<T>(0,2));
	hinv.at<T>(1,2) = determinant*(h.at<T>(0,2)*h.at<T>(1,0)-h.at<T>(1,2)*h.at<T>(0,0));

	hinv.at<T>(2,0) = determinant*(h.at<T>(1,0)*h.at<T>(2,1)-h.at<T>(2,0)*h.at<T>(1,1));
	hinv.at<T>(2,1) = determinant*(h.at<T>(0,1)*h.at<T>(2,0)-h.at<T>(2,1)*h.at<T>(0,0));
	hinv.at<T>(2,2) = determinant*(h.at<T>(0,0)*h.at<T>(1,1)-h.at<T>(1,0)*h.at<T>(0,1));

	return true;
}

/**
 *
 * @param h The homography that will be applied
 * @param origin The 2D point that will be transformed
 * @param destination The result of the transformation in a 2D point
 */
template <typename T>
void apply(const cv::Mat& h, const cv::Point_<T>& origin, cv::Point_<T>& destination) {
	double a = h.at<T> (0, 0) * origin.x + h.at<T> (0, 1) * origin.y + h.at<T> (0, 2);
	double b = h.at<T> (1, 0) * origin.x + h.at<T> (1, 1) * origin.y + h.at<T> (1, 2);
	double c = h.at<T> (2, 0) * origin.x + h.at<T> (2, 1) * origin.y + h.at<T> (2, 2);
	destination.x = (T) (a / c);
	destination.y = (T) (b / c);

}


} /* Homography */

} /* hvrl */

#endif /* _HOMOGRAPHY_HPP__ */
